﻿// Decompiled with JetBrains decompiler
// Type: DnProcessSimulateCommands.RobotJog.CApRJRobotJogMovementCache
// Assembly: DnProcessSimulateCommands, Version=2402.0.0.890, Culture=neutral, PublicKeyToken=null
// MVID: A885D881-AEDF-447E-852A-6F13102E089C
// Assembly location: C:\Program Files\Tecnomatix_2402\eMPower\DotNetCommands\DnProcessSimulateCommands.dll

using System;
using System.Collections;
using System.Collections.Generic;
using Tecnomatix.Engineering;


namespace DnProcessSimulateCommands.RobotJog;

internal class CApRJRobotJogMovementCache
{
  private const double Epsilon = 0.001;
  private readonly CApRJRobotJogManipulationLogic _manipulationLogic;
  private CApRJRobotState _cachedState;
  private CApRJRobotState _lastState;
  private string _robotConfiguration;
  private ITxLocatableObject _movingObject;
  private ITxRoboticLocationOperation _location;
  private MovementAxis _movementAxis;
  private TxVector _referenceFrameRotation;
  public TxTransformation _delta;
  private bool _isValid;

  public CApRJRobotJogMovementCache(CApRJRobotJogManipulationLogic manipulationLogic)
  {
    this._manipulationLogic = manipulationLogic;
  }

  private static bool AreEqual(double a, double b) => Math.Abs(a - b) < 0.001;

  private static bool AreEqual(TxVector a, TxVector b)
  {
    return CApRJRobotJogMovementCache.AreEqual(a.X, b.X) && CApRJRobotJogMovementCache.AreEqual(a.Y, b.Y) && CApRJRobotJogMovementCache.AreEqual(a.Z, b.Z);
  }

  private static bool AreEqual(TxTransformation a, TxTransformation b)
  {
    return CApRJRobotJogMovementCache.AreEqual(a.Translation.X, b.Translation.X) && CApRJRobotJogMovementCache.AreEqual(a.Translation.Y, b.Translation.Y) && CApRJRobotJogMovementCache.AreEqual(a.Translation.Z, b.Translation.Z) && CApRJRobotJogMovementCache.AreEqual(a.RotationRPY_XYZ.X, b.RotationRPY_XYZ.X) && CApRJRobotJogMovementCache.AreEqual(a.RotationRPY_XYZ.Y, b.RotationRPY_XYZ.Y) && CApRJRobotJogMovementCache.AreEqual(a.RotationRPY_XYZ.Z, b.RotationRPY_XYZ.Z);
  }

  private bool IsLastStateChanged(CApRJRobotState currentState)
  {
    if (!CApRJRobotJogMovementCache.AreEqual(currentState.RobotTransformation, this._lastState.RobotTransformation))
      return true;
    foreach (KeyValuePair<ITxDevice, TxPoseData> robotPose in currentState.RobotPoses)
    {
      ArrayList jointValues1 = robotPose.Value.JointValues;
      ArrayList jointValues2 = this._lastState.RobotPoses[robotPose.Key].JointValues;
      for (int index = 0; index < jointValues1.Count; ++index)
      {
        if (!CApRJRobotJogMovementCache.AreEqual((double) jointValues1[index], (double) jointValues2[index]))
          return true;
      }
    }
    return false;
  }

  private bool IsConfiguratinValid()
  {
    return !this._manipulationLogic.IsConfigurationLocked || this._manipulationLogic.SelectedConfiguration == this._robotConfiguration;
  }

  private bool IsValid(
    CApRJRobotState currentState,
    MovementAxis movementAxis,
    TxVector referenceFrameRotation)
  {
    return movementAxis == this._movementAxis && this.IsConfiguratinValid() && this._manipulationLogic.MovingObject == this._movingObject && this._manipulationLogic.Location == this._location && CApRJRobotJogMovementCache.AreEqual(referenceFrameRotation, this._referenceFrameRotation) && !this.IsLastStateChanged(currentState);
  }

  public void Update(TxTransformation delta)
  {
    this._delta = delta;
    this._lastState = this._manipulationLogic.Clipboard.GetRobotAndLocationState();
  }

  public void SetCachedState(MovementAxis movementAxis, TxVector referenceFrameRotation)
  {
    CApRJRobotState andLocationState = this._manipulationLogic.Clipboard.GetRobotAndLocationState();
    if (!this.IsValid(andLocationState, movementAxis, referenceFrameRotation))
    {
      this._cachedState = andLocationState;
      this._lastState = andLocationState;
      this._robotConfiguration = this._manipulationLogic.SelectedConfiguration;
      this._movingObject = this._manipulationLogic.MovingObject;
      this._location = this._manipulationLogic.Location;
      this._movementAxis = movementAxis;
      this._referenceFrameRotation = referenceFrameRotation;
      this._delta = new TxTransformation();
    }
    else
      this._manipulationLogic.Clipboard.SetRobotAndLocationState(this._cachedState);
  }

  public void RestorePreviousState()
  {
    this._manipulationLogic.Clipboard.SetRobotAndLocationState(this._lastState);
  }

  public TxTransformation GetCombinedDelta(TxTransformation increment)
  {
    switch (this._movementAxis)
    {
      case MovementAxis.T:
        return new TxTransformation(TxVector.op_Addition(this._delta.Translation, increment.Translation), (TxTransformation.TxTransformationType) 1);
      case MovementAxis.Rx:
        return new TxTransformation(new TxVector(this._delta.RotationRPY_XYZ.X + increment.RotationRPY_XYZ.X, 0.0, 0.0), (TxTransformation.TxRotationType) 1);
      case MovementAxis.Ry:
        return new TxTransformation(new TxVector(0.0, this._delta.RotationRPY_XYZ.Y + increment.RotationRPY_XYZ.Y, 0.0), (TxTransformation.TxRotationType) 1);
      case MovementAxis.Rz:
        return new TxTransformation(new TxVector(0.0, 0.0, this._delta.RotationRPY_XYZ.Z + increment.RotationRPY_XYZ.Z), (TxTransformation.TxRotationType) 1);
      default:
        return new TxTransformation();
    }
  }
}
